#include "getindex3d1.h"
#define PI 3.14
void getindex3d1(vector<float> xx,vector<float> yy,vector<float> th,vector<int> nz,vector<float> zmin,vector<float> zmax,vector<int> idk)
{
    float dx=(zmax[0]-zmin[0])/nz[0];
    float dy=(zmax[1]-zmin[1])/nz[1];
    float dth=(zmax[2]-zmin[2])/nz[2];
    vector<float> tth;
    for(int i=0;i<th.size();i++)
    {
        float th_temp=fmod(th[i],2*PI);
        if(th_temp>PI)
            th_temp=th_temp-2*PI;
        th_temp=th_temp-0.001*((signbit(th_temp))? -1:1)+0.0001;
        tth.push_back(th_temp);
    }
    for(int i=0;i<xx.size();i++)
    {
        int id1=ceil((xx[i]-zmin[0])/dx);
        int ir1=(id1>0&id1<nz[0])? 1:0;
        for(int j=0;j<yy.size();j++)
        {
            int id2=ceil((yy[j]-zmin[1])/dy);
            int ir2=(id1>0&id1<nz[1])? 1:0;
            for(int k=0;k<th.size();k++)
            {
                int id3=ceil((tth[k]-zmin[2])/dth);
                int ir3=(id2>0&id2<nz[2])? 1:0;
                int k_temp;
                if(ir1*ir2*ir3==0)
                {
                    k_temp=1;
                }
                else
                {
                    k_temp=id1+(id2-1)*nz[0]+(id3-1)*nz[1];
                }
                idk.push_back(k_temp);
            }
        }
    }



}